/* The is the Rover Team Project Code for The California Institute of Technology 2012.
Copyright (C) 2012 scchang, bryanhe, dflicker

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA. */
#ifndef INPUT_STATE_
#define INPUT_STATE_

#include "include/input_state.h"

InputState::InputState(void){
  bzero(comps, NUM_INPUT_COMS);
}

unsigned char InputState::getValue(InputState_Component comp){

  // Sanity Check
  int intValue = (int)comp;
  if(intValue < 0 || intValue >= NUM_INPUT_COMS){
    fprintf(stderr, "In InputState::getValue, invalid InputState_Component.\n");
    return (unsigned char)128;
  }

  return this->comps[intValue];
}

int InputState::getStringLength(void){
  return (2 * NUM_INPUT_COMS) + 1;
}

int InputState::setValue(InputState_Component comp, unsigned char value){

  // Sanity Check
  int intValue = (int)comp;
  if(intValue < 0 || intValue > NUM_INPUT_COMS){
    fprintf(stderr, "In InputState::setValue, invalid InputState_Component.\n");
    return -1;
  }
  this->comps[intValue] = value;
  return 0;
}

int InputState::toString(unsigned char** buffer){
  // We add two for the inputs
  int size = (2 * NUM_INPUT_COMS) + 1;
  unsigned char* output = new unsigned char[size];
  
  // Buffer Zero
  bzero(output, size);
  
  // Endpoints
  output[0] = STRING_STAR_POINT;
  output[size - 1] = STRING_END_POINT;

  // Add Delinators
  for(int i = 2; i < size - 1; i += 2){
    output[i] = STRING_DELINATOR;
  }

  // Now Actual Information
  int index = 0;
  for(int i = 1; i < size - 1; i += 2, index++){
    output[i] = this->comps[index];
  }

  *buffer = output;

  return 0;
}

InputState* InputState::getInputState(unsigned char* input){
  int size = (2 * NUM_INPUT_COMS) + 1;
  
  // Sanity Check
  if(check_correct_delinate(input, size) < 0){
    fprintf(stderr, "In InputState::getInputState, Incorrect Delination of Input String.\n");
    return NULL;
  }
  
  // Assuming correct delination
  int index = 0;
  InputState* output = new InputState();
  for(int i = 1; i < size - 1; i += 2, index++){
    (*output).comps[index] = input[i];
  }
  
  return output;
}

void InputState::verify(void){
  InputState input = InputState();
  unsigned char assign[NUM_INPUT_COMS];
  unsigned char* string_form;
  
  for(int i = 0; i < NUM_INPUT_COMS; i++){
    assign[i] = (unsigned char)i+200;
  }

  for(int i = 0; i < NUM_INPUT_COMS; i++){
    input.setValue((InputState_Component)i, assign[i]);
  }

  for(int i = 0; i < NUM_INPUT_COMS; i++){
    assert(input.getValue((InputState_Component)i) == assign[i]);
  }

  input.toString(&string_form);
  // String Form

  InputState* state = getInputState(string_form);
  
  for(int i = 0; i < NUM_INPUT_COMS; i++){
    assert((*state).getValue((InputState_Component)i) == input.getValue((InputState_Component)i));
  }

  delete []string_form;
}

#endif
